﻿using UnityEngine;
using System;
using System.Linq;
using System.Collections;
using System.Collections.Generic;

namespace Hont.AStar
{
    public class HontAStarHelper
    {
        public static Position[] CombinePath(Position[] positionArray)
        {
            var result = new Position[positionArray.Length];

            var length = CombinePath_NonAlloc(positionArray, positionArray.Length, ref result);

            Array.Resize(ref result, length);

            return result;
        }

        public static int CombinePath_NonAlloc(Position[] positionArray, int validLength, ref Position[] resultPositionArray)
        {
            if (validLength < 2)
            {
                for (int i = 0; i < validLength; i++)
                    resultPositionArray[i] = positionArray[i];

                return validLength;
            }

            var elementCount = 0;
            var lastDirection = (Position?)null;

            elementCount++;

            for (int i = 1; i < validLength; i++)
            {
                var x = positionArray[i - 1];
                var y = positionArray[i];
                var flag = true;

                var direction = new Position();

                direction.X = y.X - x.X;
                direction.Y = y.Y - x.Y;
                direction.Z = y.Z - x.Z;

                if (lastDirection != null && (!Mathf.Approximately(direction.X, lastDirection.Value.X) || !Mathf.Approximately(direction.Y, lastDirection.Value.Y) || !Mathf.Approximately(direction.Z, lastDirection.Value.Z)))
                    flag = false;

                if (!flag)
                {
                    resultPositionArray[elementCount] = x;
                    elementCount++;
                }

                lastDirection = direction;
            }

            resultPositionArray[0] = positionArray[0];
            resultPositionArray[elementCount] = positionArray[validLength - 1];
            elementCount++;

            return elementCount;
        }
    }
}
